﻿namespace SEED.SEEDControllers {
    using System;

    public static class Steim1 {

        public static int[] Decode(byte[] bytes, int numSamples, bool swapBytes, int bias) {
            if (bytes.Length % 64 != 0) {
                throw new ArgumentException("encoded data length is not multiple of 64 bytes (" + bytes.Length + ")");
            }

            int[] samples = new int[numSamples];
            int[] tempSamples;
            int numFrames = bytes.Length / 64;
            int current   = 0;
            int start     = 0, end = 0;
            int firstData = 0;
            int lastValue = 0;

            for (int i = 0; i < numFrames; i++) {
                tempSamples = ExtractSamples(bytes, i * 64, swapBytes);
                firstData = 0;              // d(0) is byte 0 by default

                if (i == 0) {               // special case for first frame
                    lastValue = bias;       // assign our X(-1)
                    
                    // x0 and xn are in 1 and 2 spots
                    start = tempSamples[1]; // X(0) is byte 1 for frame 0
                    end   = tempSamples[2]; // X(n) is byte 2 for frame 0
                    firstData = 3;          // d(0) is byte 3 for frame 0

                    // if bias was zero, then we want the first sample to be X(0) constant
                    if (bias == 0) {
                        // X(-1) = X(0) - d(0)
                        lastValue = start - tempSamples[3];  
                    }
                }

                for (int j = firstData; j < tempSamples.Length && current < numSamples; j++) {
                    samples[current] = lastValue + tempSamples[j];  // X(n) = X(n-1) + d(n)
                    lastValue = samples[current];
                    current++;
                }
            }   // end for each frame...

            if (current != numSamples) {
                throw new Exception("Number of samples decompressed doesn't match number in header: " + current + " != " + numSamples);
            }

            return samples;
        }

        public static int[] Decode(byte[] bytes, int numSamples, bool swapBytes) {
            // zero-bias version of decode
            return Decode(bytes, numSamples, swapBytes, 0);
        }


        private static int[] ExtractSamples(byte[] bytes, int offset, bool swapBytes) {

            int nibbles = Utility.bytesToInt(bytes[offset], bytes[offset + 1], bytes[offset + 2], bytes[offset + 3],swapBytes);
            int currNibble = 0;
            int[] temp = new int[64];  // 4 samples * 16 longwords, can't be more
            int currNum = 0;

            // i is the word number of the frame starting at 0
            for (int i = 0; i < 16; i++) {
                // count from top to bottom each nibble in W(0)
                currNibble = (nibbles >> (30 - i * 2)) & 0x03;

                switch (currNibble) {
                    case 0:
                        // only include header info if offset is 0
                        if (offset == 0) {
                            temp[currNum++] = Utility.bytesToInt(bytes[offset + (i * 4)],
                                    bytes[offset + (i * 4) + 1],
                                    bytes[offset + (i * 4) + 2],
                                    bytes[offset + (i * 4) + 3],
                                    swapBytes);
                        }
                        break;
                    case 1:
                        for (int n = 0; n < 4; n++) {
                            temp[currNum] = Utility.bytesToInt(bytes[offset + (i * 4) + n]);
                            currNum++;
                        }
                        break;
                    case 2:
                        for (int n = 0; n < 4; n += 2) {
                            temp[currNum] = Utility.bytesToInt(bytes[offset + (i * 4) + n],
                                    bytes[offset + (i * 4) + n + 1],
                                    swapBytes);
                            currNum++;
                        }
                        break;
                    case 3:
                        temp[currNum++] = Utility.bytesToInt(bytes[offset + (i * 4)],
                                bytes[offset + (i * 4) + 1],
                                bytes[offset + (i * 4) + 2],
                                bytes[offset + (i * 4) + 3],
                                swapBytes);
                        break;
                    default:
                        break;
                }
            }

            int[] output = new int[currNum];
            Array.Copy(temp, 0, output, 0, currNum);  // trim array to number of values
            return output;
        }

        public static SteimFrameBlock Encode(int[] samples, int frames, int bias) {
            if (samples.Length == 0) {
                throw new Exception("samples array is zero size");
            }
            if (frames <= 0) {
                throw new Exception("number of frames is not a positive value");
            }
            if (frames > 63) {
                throw new Exception("cannot encode more than 63 frames, you asked for " + frames);
            }

            SteimFrameBlock frameBlock = new SteimFrameBlock(frames, 1);

            frameBlock.AddEncodedWord(samples[0], 0, 0);                  // X(0) -- first sample value
            frameBlock.AddEncodedWord(samples[samples.Length - 1], 0, 0); // X(N) -- last  sample value

            int sampleIndex = 0;        // where we are in the sample array
            int[] diff = new int[4];    // store differences here
            int diffCount   = 0;        // how many sample diffs we put into current word
            int maxSize     = 0;        // the maximum diff value size encountered
            int curSize     = 0;        // size of diff value currently looked at

            while (sampleIndex < samples.Length) {
                diffCount = 0;
                maxSize = 0;

                for (int i = 0; i < 4; i++) {
                    if (sampleIndex + i < samples.Length) {
                        // as long as there are still samples
                        // get next difference  X[i] - X[i-1]
                        if (sampleIndex + i == 0) {
                            // special case for d(0) = x(0) - x(-1).
                            diff[0] = samples[0] - bias;
                        }
                        else {
                            diff[i] = samples[sampleIndex + i] - samples[sampleIndex + i - 1];
                        }
                        // and increment the counter
                        diffCount++;
                    }
                    else break;  // no more samples, leave for loop

                    if (diff[i] <= 127 && diff[i] >= -128) curSize = 1;
                    else if (diff[i] <= 32767 && diff[i] >= -32768) curSize = 2;
                    else curSize = 4;
                    // get the maximum size
                    if (curSize > maxSize) maxSize = curSize;
                    

                    if (maxSize * diffCount == 4) break;
                    else if (maxSize * diffCount > 4) {
                        diffCount--;
                        if (diffCount == 3) diffCount--;
                        break;
                    }
                }

                // generate the encoded word and the nibble value
                int nibble = 0;
                int word   = 0;
                if (diffCount == 1) {
                    word = diff[0];
                    nibble = 3;  // size 4 = 11
                }
                else if (diffCount == 2) {
                    word = (diff[0] & 0xffff) << 16;  // clip to 16 bits, then shift
                    word |= (diff[1] & 0xffff);
                    nibble = 2;  // size 2 = 10
                }
                else {  // diffCount == 4
                    word = (diff[0] & 0xff) << 24;    // clip to 8 bits, then shift
                    word |= (diff[1] & 0xff) << 16;
                    word |= (diff[2] & 0xff) << 8;
                    word |= (diff[3] & 0xff);
                    nibble = 1;  // size 1 = 01
                }

                // add the encoded word to the frame block
                if (frameBlock.AddEncodedWord(word, diffCount, nibble)) {
                    // frame block is full (but the value did get added) 
                    // so modify reverse integration constant to be the very last value added
                    // and break out of loop (read no more samples)
                    frameBlock.SetXn(samples[sampleIndex + diffCount - 1]); // X(N)
                    break;
                }

                // increment the sampleIndex by the diffCount
                sampleIndex += diffCount;

            } // end while next sample
            return frameBlock;
        }

        public static SteimFrameBlock Encode(int[] samples, int frames) {
            // zero-bias version of encode
            return Encode(samples, frames, 0);   
        }

    }
}
